
#include "RBFfunction.h"
#include <cassert>

RBFfunction::RBFfunction() {
   
   n = 0;
   npolyterms = 4; // 4 or 10
}

RBFfunction::~RBFfunction() {
   
   clear();
}

void RBFfunction::setting(const Constraints *c) {
   
   constraints = *c;
   compute_iM();
}

void RBFfunction::clear() {
   
   constraints.clear();
   n = 0;
}

void RBFfunction::polynomialDegree(unsigned int i) {
   
   switch (i) {
      case 0: npolyterms = 0;  break;
      case 1: npolyterms = 4;  break;
      case 2: npolyterms = 10; break;
   }
}

inline Number RBFfunction::phi(const Vector3& v) {
   
   const Number &sqrLen = v*v;
   //~ return sqrLen*(sqrt(sqrLen)); // phi(v)=|v|^3
   return std::sqrt(sqrLen);   // phi(v)=|v|
   
//    if (v*v == 0) return 0;
//    return (v*v)*log(sqrt(v*v));
   
//      return 1.0/sqrt(v*v+1);
//      return sqrt(v*v);

   //~ float h=1.0;
   //~ return exp(-(v*v)/(h*h));
}

/// It computes the matrix inverse
void RBFfunction::compute_iM() {
   
   if (constraints.empty())  return;
   n = constraints.size();
   
   /// Built the RBF matrix 
   unsigned int size = n+npolyterms;
   MatrixCPPL M(size, size);
   for (unsigned int row=0; row<n; ++row) {
      const Point3 Pf = constraints.getPosition(row);
      for (unsigned int col=0; col<n; ++col) {
         const Point3 Pc = constraints.getPosition(col);
         M(row,col) = phi(Pf-Pc);
      }
      for (unsigned int col=n; col<size; ++col) {
         M(row,col) = polynomial(Pf,col-n);
      }
   }
   
   /// Fill the bottom rows of the matrix
   for (unsigned int row=n; row<size; ++row) {
      for (unsigned int col=0; col<size; ++col) {
         if (col < n) M(row,col) = M(col,row);
         else         M(row,col) = 0.0;
      }
   }
   
   /// Invert the matrix
   iM = CPPL::i(M);
   //~ cout<<endl<<iM<<endl;
}

/// It computes the RBF's weights 
void RBFfunction::compute_lambdas(const Constraints *c) {
   
   if (c == 0)     return;
   //~ if (c->empty()) return;
   
   MatrixCPPL RightSide(n+npolyterms, 3);
   for (unsigned int i=0; i<n; ++i) {
      const Point3 &Q = c->getPosition(i); 
      RightSide(i,0)=Q[0];
      RightSide(i,1)=Q[1];
      RightSide(i,2)=Q[2];
   }
   
   /// additional restrictions initialization
   for (unsigned int i=0; i<npolyterms; ++i) {
      RightSide(n+i,0)=0.0;
      RightSide(n+i,1)=0.0;
      RightSide(n+i,2)=0.0;
   }
   
   lambdas = iM*RightSide;
   //~ cout<<endl<<lambdas<<endl;
}

Point3 RBFfunction::evalf(const Point3 &p) {
   
   Number rx=0, ry=0, rz=0;
   /// radial component
   for (unsigned int i=0; i<n; ++i) {
      const Point3 &P = constraints.getPosition(i);
      const Number &PHI = phi(p-P);
      rx += lambdas(i,0)*PHI;
      ry += lambdas(i,1)*PHI;
      rz += lambdas(i,2)*PHI;
   }
   /// polynomial component
   /// parte das colunas de lambdas sao os coeficientes do polinomio
   /// que corresponde ao componente x, y ou z
   for (unsigned int i=0; i<npolyterms; ++i) {
      const Number &valpoly = polynomial(p, i);
      rx += lambdas(n+i,0)*valpoly;
      ry += lambdas(n+i,1)*valpoly;
      rz += lambdas(n+i,2)*valpoly;
   }
   return Point3(rx, ry, rz);
}

Point3 RBFfunction::evalf(const Vector3 &v) {
   
   return evalf(Point3(v[0],v[1],v[2]));
}

Number RBFfunction::polynomial(const Point3& p, unsigned int i) {
   
   assert(i<10);
   Number value=0;
   switch(i) {
      case 0:  value=1;               break;
      case 1:  value=p[0];            break;
      case 2:  value=p[1];            break;
      case 3:  value=p[2];            break;
      case 4:  value=p[0]*p[1];       break;
      case 5:  value=p[0]*p[2];       break;
      case 6:  value=p[1]*p[2];       break;
      case 7:  value=p[0]*p[0];       break;
      case 8:  value=p[1]*p[1];       break;
      case 9:  value=p[2]*p[2];       break;
   }
   return value;
}
